package pathfinding;

import java.awt.geom.Point2D;
import java.io.Serializable;
import java.util.ArrayList;
import java.util.logging.Level;
import java.util.logging.Logger;

import ownserver.ServerShip;
import robotutils.data.DoubleUtils;

import base.Geometry;
import base.Timestamped;

import com.sun.sgs.app.AppContext;
import com.sun.sgs.app.ManagedObject;
import com.sun.sgs.app.ManagedReference;
import com.sun.sgs.app.Task;

public class DriveWithPathfinding implements ManagedObject, Serializable, Task {
	
	protected ManagedReference<IPathfinder<GridCell>> finderRef;
	protected ArrayList<Timestamped<GridCell>> path = null;
	ManagedReference<ServerShip> shipRef;
	GridCell goal = null;
	
	private static final Logger logger =
			Logger.getLogger(DriveWithPathfinding.class.getName());
	
	private static final double headingCloseEnough = 0.001;
	
	// index of the GridCell we're following on the path atm
	protected int currentIndex = 0;
	
	// has the path changed since we last set our driving target? 
	protected boolean pathChanged = false;
	
	/** The time to wait before repeating the task. */
    public static final int PERIOD_MS = 400;
    
    private double heading, velocity;
    
    /**  The timestamp when this task was last run. */
    private long lastTimestamp = System.currentTimeMillis();
    
    public DriveWithPathfinding(ManagedReference<IPathfinder<GridCell>> finderRef,
    		ManagedReference<ServerShip> shipRef) {
    	AppContext.getDataManager().markForUpdate(this);
    	this.finderRef = finderRef;
		this.shipRef = shipRef;
		
		// magic ships can instantly come to full stop.
		ServerShip ship = shipRef.get();
		ship.setAcceleration(0);
		ship.setRotation(0);
		heading = ship.getHeading();
		velocity = ship.getVelocity();
	}
    
	public void run() throws Exception {
		long timestamp = System.currentTimeMillis();
        long delta = timestamp - lastTimestamp;
        
        lastTimestamp = timestamp;
        
        if (shipRef == null) {
			return;
		}
        
        double dt = ((double)delta) / 1000;
        
        double plannedX = shipRef.get().getPlannedX(), plannedY = shipRef.get().getPlannedY();
        double x = shipRef.get().getX(), y = shipRef.get().getY();
        
        updatePathIfNeeded(plannedX, plannedY, x, y, dt);
        
        // getting the ship only here is intentional. this way ship is not reserved during the pathfinding 
        ServerShip s = shipRef.get();
        drive(s, dt);
	}
	
	private static final long serialVersionUID = 1L;

	protected void updatePathIfNeeded(double plannedX, double plannedY, double x, double y, double dt)
	{
		if (Double.isNaN(plannedX) || Double.isNaN(plannedY) ) {
			return;
		}
		
		boolean update = false;
		if (goal == null || GridGeometry.equalCells(goal, plannedX, plannedY) == false)
		{
			// update if goal of the ship has changed
			goal = GridGeometry.getCell(plannedX, plannedY);
			update = true;
			//logger.log(Level.INFO, "Updating because of changed goal.");
		}
		else if (finderRef.get().needsUpdate()) {
			// update if pathfinder thinks update is needed
			update = true;
			//logger.log(Level.INFO, "Updating because of changed environment.");
		}
		
		if (update)
		{
			AppContext.getDataManager().markForUpdate(this);
			path = finderRef.get().getPath(GridGeometry.getCell(x, y), goal);
			pathChanged = true;
		}
	}
		
	protected void drive(ServerShip s, double dt)
	{
		if (path == null || path.isEmpty()) {
			updateVelocity(s, 0);
			//logger.log(Level.INFO, "Empty path! This is not cooperative!");
			return;
		}
		
		if (pathChanged)
		{
			currentIndex = 0;
			pathChanged = false;
		}
		
		int newIndex = getCurrentIndexInPath();
		
		if (newIndex != currentIndex) {
			//logger.log(Level.INFO, "Index updated to " + newIndex);
			currentIndex = newIndex;
			AppContext.getDataManager().markForUpdate(this);
		}
		
		if (currentIndex >= path.size()) {
			return;
		}
		  
		drive(s, path.get(currentIndex).thing);
	}
	
	/**
	 * Drives the ship directly to the center of the grid, and once there, stops. 
	 * 
	 * @param ship
	 * @param target
	 */
	protected void drive(ServerShip ship, GridCell target)
	{
		//logger.log(Level.INFO, "Heading to " + target + " from " + GridGeometry.getCell(ship) + " = " + GridGeometry.getCell(ship.getX(), ship.getY()));
		
		// TODO: this should be updated when we get ships of various sizes
		double closeEnoughSq = GridGeometry.gridSize / 30.0;  //Math.min(ServerShip.getWidth(), ServerShip.getLength()) / 3.0;
		closeEnoughSq *= closeEnoughSq;
		
		Point2D.Double tgtPos = GridGeometry.getCenter(target);
		Point2D.Double shipPos = ship.getPos();
		
		//logger.log(Level.INFO, "Heading to " + tgtPos + " from " + shipPos);
		
		double distSq = Geometry.getDistanceSquared(shipPos, tgtPos);
		
		if (distSq <= closeEnoughSq) {
			updateVelocity(ship, 0);
//			logger.log(Level.INFO, ship + " reached close enough to grid center, stopping. Sq dist {0} vs. {1}",
//					new Object[] {distSq, closeEnoughSq});
			return;
		}
		
//		logger.log(Level.INFO, "{0} heading from ({1}, {2}) to ({3}, {4}).", 
//				new Object[]{ship, shipPos.x, shipPos.y, tgtPos.x, tgtPos.y});
		
		// if the ship goes at max speed when getting near to it's target it might overshoot
		double distInPeriod = PERIOD_MS / 1000.0 * ship.getMaxVelocity();
		double distInPeriodSq = distInPeriod * distInPeriod;
		double velocityModifier = distSq / distInPeriodSq;
		if (velocityModifier > 1.0) {
			velocityModifier = 1.0;
		}
		
		updateHeading(ship, Geometry.getHeading(shipPos.x, shipPos.y, tgtPos.x, tgtPos.y));
		updateVelocity(ship, ship.getMaxVelocity() * velocityModifier);
	}
	
	private void updateVelocity(ServerShip s, double newVelocity)
	{
		if (DoubleUtils.equals(newVelocity, velocity) == false) {
			AppContext.getDataManager().markForUpdate(this);
			velocity = newVelocity;
			//logger.log(Level.INFO, s + " changed velocity to " + velocity);
			s.setVelocity(velocity);
		}
	}
	
	private void updateHeading(ServerShip s, double newHeading) {
		//if (DoubleUtils.equals(newHeading, heading) == false) {
		if (Math.abs(newHeading - heading) > headingCloseEnough) {
			AppContext.getDataManager().markForUpdate(this);
			heading = newHeading;
			//logger.log(Level.INFO, s + " changed heading to " + heading);
			s.setHeading(heading);
		}
	}
	
	protected int getCurrentIndexInPath()
	{
		if (path.size() == 1) {
			return 0;
		}
		long timenow = System.currentTimeMillis();
		int result = currentIndex; 
		while (result < path.size() -1 &&
			   path.get(result).timestamp <= timenow)
		{
			result++;
		}
		
//		if (result != currentIndex) {
//			logger.log(Level.INFO, "Switched index from {0} to ({1}", new Object[]{currentIndex, result});
//		}
//		else
//		{
//			logger.log(Level.INFO, "Kept index as {0}", result);
//		}
		 
		return result;
	}
}
